#include <GL/glut.h>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>

#define TIME 5000
#define NUM 250 
#define ACC 5000
#define DEV /dev/ttyACM0
#define MAXSLOPE 0.017// разность скоростей, больше которой считать, что начилось движение
#define POINTS 3//по скольким точкам подсчитывать разность

FILE *comm, *check,*logg,*outf;

bool prevstate;
struct carrier
{
	float time[NUM];// время в мс
	float acc[NUM]; // ускорение в 1000*м/с^2
	float speed[NUM];//скорость в м/с
	float coord[NUM]; //координата в метрах от начальной точки
	float correction;
	int startI,stopI;
	int i;
	int prevn;
	int reopen;
	bool state(int curn, int nump) //возвращает 0, если платформа остановлена, 1 если движется
//	float state(int curn, int nump) 
	{
		float dels=0,delt=0, slope=0;
		for(int i=curn; i>=curn-nump; i--)
		{
			dels=dels+1000*(speed[i]-speed[i-1]);
			delt=delt+1000*(time[i]-time[i-1]);
//			slope=slope+(1000*dels/delt);
		}
		slope=fabs(1000*dels/delt)/nump; //немного нечестное усреднение но работает лучше, чем честное
		if (slope<=MAXSLOPE)
			{return 0;}
		else
			{return 1;};
	}
	int substract()
	{
//	logg=fopen("outf","w");
		float b, k;
		k=(speed[stopI]-speed[startI])/(time[stopI]-time[startI]);
		b=speed[startI]-k*time[startI];
		for(int i=startI;i<=stopI+1;i++)
		{
			speed[i]=speed[i]-(k*time[i]+b);	
		}
		
//		fprintf(logg,"start %f stop %f\n",time[startIf],time[stopIf]);
		return 0;
	}
	void reset()
	{
		for(int i=0;i<NUM;i++)
		{
		time[i]=acc[i]=speed[i]=coord[i]=0;
		}
	}
};
struct carrier mainD;

void wndowinit()
{
  glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB);
  glutInitWindowSize(800, 600);
  glutInitWindowPosition(100, 100);
  glutCreateWindow("graph");
  glClearColor(1.0, 1.0, 1.0, 1.0);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(0.0, TIME, 0.0, ACC, -1.0, 1.0);
}



void display()
{
  glClear(GL_COLOR_BUFFER_BIT);
  //ось TIME
  glBegin(GL_LINES);	
  glColor3f(0.0, 0.0, 0.0);	      
		glVertex2f(0,ACC/2);
		glVertex2f(TIME,ACC/2);
	glEnd();

  glBegin(GL_LINE_STRIP);
  glColor3f(1.0, 0.0, 0.0);
  for (int i = 1; i < NUM-50; i++)
  {

//			glVertex2f(mainD.time[i],1000*mainD.coord[i]+ACC/2);
			glVertex2f(mainD.time[i],1000*mainD.speed[i]+ACC/2);
//			glVertex2f(mainD.time[i],1000*mainD.acc[i]+ACC/2);
		
//		glVertex2f(i,rand()%(ACC/2)+ACC/2);
  }
  glEnd();
  glutSwapBuffers();
}


int restartMC() //сброс цикла
{
    comm=fopen("/dev/ttyACM0", "w");
    fprintf(comm,"%i \n",1);
    fclose(comm);
    usleep(60*1000);
//    mainD.correction=correct(10);
}

void cleardisp()
{
	glClear(GL_COLOR_BUFFER_BIT);
	glBegin(GL_LINES);
	glColor3f(0.0, 0.0, 0.0);      
		glVertex2f(500, 5);
  	glVertex2f(500, 0);
	glEnd();
	for(int i=0; i<NUM-50; i++)
	{
		mainD.acc[i]=mainD.time[i]=0;
	}
	glutSwapBuffers();
}
float correct(int num)
{
    int i,n,t;
    float x,y,z,res;
    for(int i=0; i<5; i++) //отброска мусора
    {
        usleep(30*1000);
        fscanf(comm,"%i %i %f %f",&n, &t, &x, &y);
    };
    for(int i=0; i<num; i++) // собственно усреднение
    {
        usleep(30*1000);
        fscanf(comm,"%i %i %f %f",&n, &t, &x, &y);
        res=res+pow((x*x+y*y),0.5);
//        fprintf(outf,"correction.. %i % \n",100*i/num);
    };
    res=res/(num);
    return res;
}

void timer(int = 0)
{
	int  n, buftime, defected;
	bool state; //состояние платормы 0 -- остановлена, 1 -- движется
	float x,y;
	float bufacc;
	float S;//площадь под кусочком графика
	comm=fopen("/dev/ttyACM0", "rt");//открытие файла каждый раз заново препятствует образованию очереди
	fscanf(comm,"%i %i %f %f",&n, &buftime, &x, &y);
	bufacc=pow((x*x+y*y),0.5);
//	fprintf(outf,"%i %i %0.0f \n",n,buftime,bufacc);
	if(abs(mainD.prevn-n)<5)//проверка потока
	{
		mainD.i++;
		mainD.prevn=n;
//		mainD.defected=0;
		mainD.time[mainD.i]=buftime;																											//время
		
   	mainD.acc[mainD.i]=9.8*bufacc/255;                                 
  	mainD.acc[mainD.i]=(mainD.acc[mainD.i]-9.8*mainD.correction/255);  
  	if(x<0){mainD.acc[mainD.i]=(mainD.acc[mainD.i])*(-1);};														//ускорение
  	
  	S=(mainD.time[mainD.i]-mainD.time[mainD.i-1])*(mainD.acc[mainD.i-1]+mainD.acc[mainD.i])/2; //скорость
  	mainD.speed[mainD.i]=mainD.speed[mainD.i-1]+S/1000; 
  	

																																									//лог
//   	fprintf(outf,"%i %i %0.0f mainD.prevn %i\n",n,buftime,bufacc, mainD.prevn);       
// 		fprintf(logg,"%i %i %0.2f %0.2f %0.2f %0.4f \n",n, t, x, y,z, mainD.acc[mainD.i]);
		if(mainD.i>=3) {state=mainD.state(mainD.i,POINTS);};
    if ((state!=prevstate)&&(state==1)) //поехали МОЗГАМИ!11ТУТУТУТУТУУ11111
    {
    	prevstate=state;
    	mainD.startI=mainD.i-1;	
    };
    if ((state!=prevstate)&&(state==0)) //остановились
    {
    	prevstate=state;
    	mainD.stopI=mainD.i;
			mainD.substract();
    };

//  			fprintf(logg,"%0.0f %0.6f %0.6f %0.6f %i \n",mainD.time[mainD.i-3],mainD.acc[mainD.i-3],mainD.speed[mainD.i-3],mainD.coord[mainD.i-3],state);
//    fprintf(logg,"%0.0f %0.6f %0.6f %0.6f %i ",mainD.time[mainD.i-3],mainD.acc[mainD.i-3],mainD.speed[mainD.i-3],mainD.coord[mainD.i-3],state); 
    
    
//    fprintf(logg,"\n");
	
	
	}
	
//	else	{mainD.defected++;};
	if( (buftime>=TIME-100) )//выход за границу TIME
	{
		for(int i=0;i<NUM+1000;i++)//специально вызываем ошибку, чтобы выйти из цикла)
		{
			S=(mainD.time[i]-mainD.time[i-1])*(mainD.speed[i-1]+mainD.speed[i])/2; //координата
  		mainD.coord[i]=mainD.coord[i-1]+S/1000; //делим на 1000 тк время в мс
			fprintf(logg,"%0.0f %0.6f %0.6f %0.6f \n",mainD.time[i],mainD.acc[i],mainD.speed[i],mainD.coord[i]);
		}
		cleardisp();
  	restartMC();
  	mainD.reset();
  	mainD.prevn=0;
    mainD.i=0;
    fprintf(logg,"time acc speed coord \n");
	};
  display();
  
  glutTimerFunc(40, timer, 0);
}


int main(int argc, char **argv)
{
float bufcor;
  //сброс цикла
	restartMC();
  comm=fopen("/dev/ttyACM0", "rt"); //открытие файла каждый раз заново препятствует образованию очереди но теряет данные
  outf= stdout;
  logg=fopen("outf","w");
  mainD.correction=0;
	mainD.correction=correct(10);

	bufcor=correct(10);
  fprintf(outf,"Корректрировка завершена. поправка %0.2f по %i значениям\n",bufcor,50);
  restartMC();
  mainD.speed[0]=0;
  fprintf(logg,"time acc speed coord \n");
  glutInit(&argc, argv);
	wndowinit();
  glutDisplayFunc(display);
  timer();
  glutMainLoop();
}
